#include "robotfinder.hpp"
#include <opencv2/opencv.hpp>
#include "timer_custom.h"


#ifndef check
#define check() assert(glGetError() == 0)
#endif //check()





int main(int argc, char ** argv){
    
    Finder finder; 

    

	time_custom time_start = getTimeNow();
    double time_total_s = 0;
    for (int i = 0; i < 100;i++){
    vector<Robot> bots = finder.getRobotPositions();
    for (size_t j = 0; j < bots.size(); j++){
        cout << bots[j].label << ": " << bots[j].position.radius_cm 
             << bots[j].position.angle_rad << std::endl;
    }
    imshow("Im",finder.getCurrentImage());
    waitKey(0);
    //read current time
	
	time_total_s = diff(getTimeNow(),time_start);
	time_start = getTimeNow();

	//print frame rate
    //std::cout << time_total_s << " s ";
	double fr = 1 / time_total_s;
	if((i%30)==0 || i == 99)
	{
		std::cout << "Framerate: " << fr << " \n";

	}
    
}


    //std::cout << outBGR;
    //imshow("currentGBR",outBGR);	
    //cv::waitKey(0);
    //std::cout << numRead << std::endl;

}




